
#include "Gcode.h"

unsigned char gBuffer::check(void)
{
	return(_records);
}

gRecord::gRecord()
{
	init();
}

void gRecord::init(void)
{
	_next = 0;
	_params=0;
	_g = 0;
	_m = 0;
	_x = 0;
	_y = 0;
	_z = 0;
	_e = 0;
	_f = 0;
	_p = 0;
}

void gRecord::parse(char *str)
{
	_params = 0;
	while(*str)
	{
		switch(*str)
		{
		case 'G':
			_g = atof(str+1);
			_params |= _G;
			break;
		case 'X':
			_x = atof(str+1);
			_params |= _X;
			break;
		case 'Y':
			_y = atof(str+1);
			_params |= _Y;
			break;
		case 'Z':
			_z=atof(str+1);
			_params |= _Z;
			break;
		case 'F':
			_f=atof(str+1);
			_params |= _F;
			break;
		case 'E':
			_e = atof(str+1);
			_params |= _E;
			break;
		case 'S':
			_s=atof(str+1);
			_params |= _S;
			break;
		case 'T':
			_t=atof(str+1);
			_params |= _T;
			break;
		case 'P':
			_p=atof(str+1);
			_params |= _P;
			break;
		case 'M':
			_m=atof(str+1);
			_params |= _M;
			break;
		}
		str++;
	}	
}

double gRecord::get(gfunc f)
{
	switch(f)
	{
	case G:
		return _g;
		break;
	case X:
		return _x;
		break;
	case Y:
		return _y;
		break;
	case Z:
		return _z;
		break;
	case F:
		return _f;
		break;
	case E:
		return _e;
		break;
	case S:
		return _s;
		break;
	case T:
		return _t;
		break;
	case P:
		return _p;
		break;
	case M:
		return _m;
		break;
	}
	return(-1);
}

int gRecord::getb(gfunc f)
{
	switch(f)
	{
	case G:
		return _params & _G;
		break;
	case X:
		return _params & _X;
		break;
	case Y:
		return _params & _Y;
		break;
	case Z:
		return _params & _Z;
		break;
	case F:
		return _params & _F;
		break;
	case E:
		return _params & _E;
		break;
	case S:
		return _params & _S;
		break;
	case T:
		return _params & _T;
		break;
	case P:
		return _params & _P;
		break;
	case M:
		return _params & _G;
		break;
	}
	return 0;
}


gRecord* gRecord::next(void)
{
	return(_next);
}

void gRecord::next(gRecord *ptr)
{
	_next = ptr;
}

gBuffer::gBuffer()
{
	_records = 0;
	_recordsMax = 20;
	_first = 0;
}

void gBuffer::add(unsigned char *str)
{
	gRecord *rcd;
	gRecord *ptr;
	
	rcd = (gRecord *) malloc(sizeof(gRecord));
	rcd->init();
	rcd->parse((char *)str);

	if(_first == 0)
		_first = rcd;
	else
	{
		ptr=_first;
		while(ptr->next())
			ptr = ptr->next();
		ptr->next(rcd);
	}
	_records++;
}

gRecord * gBuffer::get(void)
{
	gRecord *ptr;
	
	if(_first == 0)
		return 0;
		
	ptr = (gRecord *)malloc(sizeof(gRecord));
	memcpy(ptr, _first, sizeof(gRecord));
	del();
	
	_records--;
	
	return ptr;
}

gRecord * gBuffer::peek(void)
{
	return(_first);
}

void gBuffer::del(void)
{
	gRecord *ptr;
	
	if(_first==0)
		return;

	ptr=_first;
	_first = _first->next();
	memset(ptr,0,sizeof(gRecord));
	free(ptr);
}

unsigned char gBuffer::available(void)
{
	return (_recordsMax - _records);
}

unsigned char gBuffer::empty(void)
{
	if(!_records)
		return 1;
	return 0;
}

gEngine::gEngine()
{
	_finished = false;
	_xStepsMM=1;
	_yStepsMM=1;
	_zStepsMM=1;	
	_eStepsMM=1;
}

void gEngine::setScale(double x, double y, double z, double e)
{
	_xStepsMM = x;
	_yStepsMM = y;
	_zStepsMM = z;	
	_eStepsMM = e;
}

void gEngine::cue(unsigned char *str)
{
	_buff.add(str);
}

void gEngine::process(void)
{
	gRecord *rcd;
	int i;
	
	
	_reply[0]=0; //wipe the reply string

	//if the process is paused
	if(_paused == true)
	{
		if(millis() < _pauseTS)
			return;
		_paused = false;  //disable the pause
	}
	
	//update the bed and if it is busy
	if(_bed->busy())
		return;
	
	//get the next gcode record
	rcd = _buff.get();
	
	if(rcd)
	{
		_finished = false;
		if(rcd->getb(G))
			processG(rcd);

		if(rcd->getb(M))
			processM(rcd);

		if(rcd->getb(T))
			processT(rcd);
		free(rcd);	
		return;
	}
	
	if((_paused==false) && (_finished==false))	
	{
		sprintf((char *)_reply, "ok\n");
		_finished = true;
	}
	return;
}

void gEngine::processG(gRecord *rcd)
{
	double x,y,z,e,f;

	if(rcd->getb(X))
		x = rcd->get(X) * _xStepsMM;
	else
		x = _bed->_x->getPosition();
		

	if(rcd->getb(Y))
		y = rcd->get(Y) * _yStepsMM;
	else
		y = _bed->_y->getPosition();

	if(rcd->getb(Z))
		z = rcd->get(Z) * _zStepsMM;
	else
		z = _bed->_z->getPosition();

	if(rcd->getb(E))
		e = rcd->get(E) * _eStepsMM;
	else
		e = _bed->_e->getPosition();

		f = rcd->get(F);

	switch((unsigned long)(rcd->get(G)))
	{
		case 0://rapid move
				_bed->move(x, y, z, e, f,10000);
			break;
		case 1://controlled move
				if(x==0 && y==0 && z==0 && e==0)
					return;
				_bed->move(x, y, z, e, f,10000);
			break;
		case 4://some kind of millis pause routine that wont hang the process;
				_pauseTS = millis() + rcd->get(P);
				_paused = true;
			break;
		case 20://set base units to inches
			break;
		case 21://set base units to mm
			break;
		case 28://move to origin
				if(!rcd->getb(X) && !rcd->getb(Y) && !rcd->getb(Z))
				{
					_bed->zero(2000,2000,2000,100);
					break;
				}

				if(rcd->getb(X))				
					_bed->_x->zero(2000,10);
				
				if(rcd->getb(Y))				
					_bed->_y->zero(2000,10);
				
				if(rcd->getb(Z))				
					_bed->_z->zero(2000,10);
				
			break;
		case 90://set absolute positioning
			break;
		case 91://set relative positioning
			break;
		case 92://define current absolute positioning
				if(rcd->getb(X))
					_bed->_x->setPosition(rcd->get(X)*_xStepsMM);

				if(rcd->getb(Y))
					_bed->_y->setPosition(rcd->get(Y)*_yStepsMM);

				if(rcd->getb(Z))
					_bed->_z->setPosition(rcd->get(Z)*_zStepsMM);

				if(rcd->getb(E))
					_bed->_e->setPosition(rcd->get(E)*_eStepsMM);
			break;
		default:
			break;
	}
}

void gEngine::processM(gRecord *rcd)
{
	switch(abs(rcd->get(M)))
	{
		case 0://Stop
			break;
		
		case 104://Set current extruder temp
			//extruder.temp(rcd->get(S));
			break;
		case 105://Request current extruder temp and base temp
			//?
			break;
		case 106://Turn cooling fan off
			//?
			break;
		case 107://Turn cooling fan on
			//?
			break;
		case 108://Set extruder motor speed
			//depreciated...
			break;
		case 110://Set current line number to Nxxx value
			//jump....
			break;
		case 111://Set debug bitfield to value of Snnn
			//
			break;
		case 112://Emergency stop
				_bed->_x->drive.set(DISABLE);
				_bed->_y->drive.set(DISABLE);
				_bed->_z->drive.set(DISABLE);
				_bed->_e->drive.set(DISABLE);
			break;
		case 113://Set extruder PWM
			//pwm(rcd->get(S));
			break;
		case 114://Get current position X,Y,Z,E
			//
			break;
		case 115://Get firmware version
			//
			break;
		case 116://Wait for all temps and slow changing values to reach their temps
			//
			break;
		case 117://Get Zero position, return X,Y,Z values after hitting endstops
			//
			break;
		case 126://Open valve and wait for N ms
			//
			break;
		case 127://Close valve and wait for N ms
			//
			break;
		case 140://Set bed temp
			//
			break;
		case 141://Set chamber temp
			//
			break;
		case 142://set holding pressure of bed (?)
			//
			break;
		case 226://Gcode initiated pause
			//
			break;
		case 227://Enable automatic reverse and prime using steps
			//extruder->rPrime(rcd->get(P),rcd->get(S));
			break;
		case 228://Disable automatic reverse and prime
			//extruder->nPrime(DISABLE);
			break;
		case 229://Enable automatic reverse and prime using rotations
			//
			break;
		case 230://Disable/Enable wait for temp change
			//
			break;
	}	
}

void gEngine::processT(gRecord *rcd)
{
	//extruder = tool[rcd->get(T)];
}